#!/usr/bin/env python3
# -------------- server_pursuit_game.py -----------
# 完美信息围捕游戏服务器
# author: CeasarSmj
# 2025-7
import socket
import threading
import numpy as np
import time
import os
import struct
import zlib
import logging
import binascii
from math import dist
import networkx as nx

# ================ 全局开关 ================
IF_VISUALIZE = True  # 是否启动Qt可视化窗口
PREDICT_STEP = 3  # 预测步长
NAV_GOAL_MAX_INTERVAL = 30  # 最大航点发送间隔(秒)
# ================ 通信协议配置 ================
FRAME_SIZE = 32  # 固定帧大小
VERSION = 0x02  # 协议版本
# 指令类型
COMMAND_TYPES = {
    "ConnectionRequest": 0x01,  # 连接请求 (客户端→服务端)
    "ConnectionResponse": 0x02,  # 连接响应 (服务端→客户端)
    "PositionReport": 0x03,  # 位置报告 (客户端→服务端)
    "NavigationGoal": 0x04,  # 导航目标 (服务端→客户端)
    "Heartbeat": 0x05,  # 心跳包 (双向)
    "CommandAck": 0x06,  # 指令确认 (双向)
    "EmergencyStop": 0x07,  # 紧急停止 (服务端→客户端)
    "StatusReport": 0x08,  # 状态报告 (客户端→服务端)
    "RoleAssignment": 0x09,  # 角色分配 (服务端→客户端)
}
# 状态码定义
STATUS_CODES = {
    "Success": 0x00,
    "GeneralError": 0x01,
    "InvalidCommand": 0x02,
    "CRCError": 0x03,
    "Timeout": 0x04,
    "RobotBusy": 0x05,
}
# 角色定义
ROLES = {"evader": 0x01, "pursuer": 0x02}
# 服务器配置
SERVER_IP = "0.0.0.0"  # 监听所有网络接口
SERVER_PORT = 6001
MAX_ROBOTS = 3
ROBOT_TIMEOUT = 8.0
HEARTBEAT_INTERVAL = 2.0
INF = 255
# 配置日志
logging.basicConfig(
    level=logging.INFO,
    format="%(asctime)s - %(levelname)s - %(message)s",
    handlers=[logging.StreamHandler(), logging.FileHandler("pursuit_game_server.log")],
)
logger = logging.getLogger(__name__)


class GraphManager:
    def __init__(self):
        self.node_coordinates = []  # 节点坐标列表
        self.adj_matrix = []  # 邻接矩阵
        self.dist_3d = []  # 三维距离矩阵
        self.f_matrix = []  # Floyd-Warshall 最短路径矩阵
        self.node_num = 0
        # 加载图数据
        self.load_graph_data()

    def load_graph_data(self):
        """加载图数据（邻接矩阵和预计算的距离矩阵）"""
        root = "../datas/"
        try:
            # 加载邻接矩阵
            adj_matrix_path = os.path.join(root, "adj_matrix.txt")
            if not os.path.exists(adj_matrix_path):
                raise FileNotFoundError(f"邻接矩阵文件不存在: {adj_matrix_path}")
            with open(adj_matrix_path, "r") as f:
                self.node_num = int(f.readline().strip())
                self.adj_matrix = np.zeros((self.node_num, self.node_num), dtype=int)
                for i in range(self.node_num):
                    row = list(map(int, f.readline().split()))
                    self.adj_matrix[i] = row
            # 加载节点坐标
            points_path = os.path.join(root, "points.txt")
            self.load_node_coordinates(points_path)
            # 加载预计算的距离矩阵 - 仅使用NPY格式
            dist_npy_path = os.path.join(root, "D_bin.npy")
            if not os.path.exists(dist_npy_path):
                raise FileNotFoundError(f"距离矩阵文件不存在: {dist_npy_path}")
            # 加载NPY格式
            start_time = time.time()
            self.dist_3d = np.load(dist_npy_path)
            load_time = time.time() - start_time
            # 验证矩阵形状
            if self.dist_3d.shape != (
                self.node_num + 1,
                self.node_num + 1,
                self.node_num + 1,
            ):
                raise ValueError(
                    f"距离矩阵形状不匹配: 预期({self.node_num},{self.node_num},{self.node_num}), "
                    f"实际{self.dist_3d.shape}"
                )
            logger.info(
                f"从NPY文件加载距离矩阵成功! 节点数: {self.node_num}, 耗时: {load_time:.4f}秒"
            )
            # 计算最短路径矩阵 (Floyd-Warshall)
            logger.info("开始计算最短路径矩阵...")
            self.compute_shortest_paths()
        except Exception as e:
            logger.error(f"加载图数据错误: {e}")
            raise  # 重新抛出异常以便上层处理

    def load_node_coordinates(self, filename):
        """从文件加载节点坐标，文件不存在时抛出异常"""
        if not os.path.exists(filename):
            raise FileNotFoundError(f"节点坐标文件不存在: {filename}")
        try:
            self.node_coordinates = []
            with open(filename, "r") as f:
                for line in f:
                    # 尝试两种格式：逗号分隔或空格分隔
                    if "," in line:
                        parts = line.strip().split(",")
                    else:
                        parts = line.strip().split()
                    if len(parts) >= 2:
                        x = float(parts[0])
                        y = float(parts[1])
                        self.node_coordinates.append((x, y))
            # 验证节点数量是否匹配
            if len(self.node_coordinates) != self.node_num:
                raise ValueError(
                    f"坐标文件节点数不匹配: 坐标文件 {len(self.node_coordinates)} 个节点, "
                    f"邻接矩阵 {self.node_num} 个节点"
                )
            logger.info(f"成功加载 {len(self.node_coordinates)} 个节点的坐标")
            return True
        except Exception as e:
            logger.error(f"加载节点坐标错误: {e}")
            raise  # 重新抛出异常

    def compute_shortest_paths(self):
        """
        使用 NetworkX 的 floyd_warshall_numpy 加速最短路径矩阵计算
        """
        # 1. 从邻接矩阵构建有向图
        G = nx.from_numpy_array(self.adj_matrix, create_using=nx.DiGraph())
        # 2. 使用 NetworkX 的 floyd_warshall_numpy 计算所有节点对的最短路径
        dist_matrix = nx.floyd_warshall_numpy(G)
        # 3. 定义不可达距离（原代码中为 255）
        INF = 255
        # 4. 替换不可达路径为 INF，并四舍五入为整数
        dist_matrix[np.isinf(dist_matrix)] = INF
        self.f_matrix = np.rint(dist_matrix).astype(int)
        # 5. 确保对角线为 0（自己到自己的距离）
        np.fill_diagonal(self.f_matrix, 0)
        logger.info("使用 NetworkX 成功计算最短路径矩阵")

    def position_to_node(self, x, y):
        """将实际坐标映射到最近的节点"""
        min_dist = float("inf")
        closest_node = 0
        for idx, (nx, ny) in enumerate(self.node_coordinates):
            d = dist((x, y), (nx, ny))
            if d < min_dist:
                min_dist = d
                closest_node = idx
        return closest_node

    def node_to_position(self, node_id):
        """将节点ID转换为实际坐标"""
        if 0 <= node_id < len(self.node_coordinates):
            return self.node_coordinates[node_id]
        return (0.0, 0.0)  # 默认位置

    def get_neighbors(self, node_id):
        """获取节点的邻居（不包括自身）"""
        neighbors = []
        for j in range(self.node_num):
            if self.adj_matrix[node_id][j] == 1:
                neighbors.append(j)
        return neighbors


class GameStrategy:
    def __init__(self, graph, pred_step=PREDICT_STEP):
        self.graph = graph
        self.capture_count = 0
        self.predict_step = pred_step

    def calculate_next_move(self, evader_node, pursuer1_node, pursuer2_node):
        if self.is_captured(evader_node, pursuer1_node, pursuer2_node):
            return evader_node, pursuer1_node, pursuer2_node

        evader_neighbors = self.graph.get_neighbors(evader_node)
        p1_neighbors = self.graph.get_neighbors(pursuer1_node)
        p2_neighbors = self.graph.get_neighbors(pursuer2_node)

        # 逃脱者决策：最大化最小距离
        best_evader = evader_node
        max_min_value = -1
        best_evader_dist_sum = float("-inf")  # 距离和初始值

        for e_next in evader_neighbors:
            min_value = float("inf")
            for p1_next in p1_neighbors:
                for p2_next in p2_neighbors:
                    value = self.graph.dist_3d[p1_next + 1][p2_next + 1][e_next + 1]
                    if value < min_value:
                        min_value = value

            # 平局处理：计算路径距离和
            dist_sum = (
                self.graph.f_matrix[pursuer1_node][e_next]
                + self.graph.f_matrix[pursuer2_node][e_next]
            )

            if min_value > max_min_value:
                max_min_value = min_value
                best_evader = e_next
                best_evader_dist_sum = dist_sum
            elif min_value == max_min_value and dist_sum > best_evader_dist_sum:
                best_evader = e_next
                best_evader_dist_sum = dist_sum

        # 追捕者决策：最小化最大距离
        best_p1 = pursuer1_node
        best_p2 = pursuer2_node
        min_max_value = float("inf")
        best_pursuer_dist_sum = float("inf")  # 距离和初始值

        for p1_next in p1_neighbors:
            for p2_next in p2_neighbors:
                max_value = -1
                for e_next in evader_neighbors:
                    value = self.graph.dist_3d[p1_next + 1][p2_next + 1][e_next + 1]
                    if value > max_value:
                        max_value = value

                # 平局处理：计算路径距离和
                dist_sum = (
                    self.graph.f_matrix[p1_next][evader_node]
                    + self.graph.f_matrix[p2_next][evader_node]
                )

                if max_value < min_max_value:
                    min_max_value = max_value
                    best_p1 = p1_next
                    best_p2 = p2_next
                    best_pursuer_dist_sum = dist_sum
                elif max_value == min_max_value and dist_sum < best_pursuer_dist_sum:
                    best_p1 = p1_next
                    best_p2 = p2_next
                    best_pursuer_dist_sum = dist_sum

        return best_evader, best_p1, best_p2

    def is_captured(self, evader_node, pursuer1_node, pursuer2_node):
        """检查逃逸者是否被捕获"""
        # 条件1: 逃逸者与任一追捕者在同一节点
        if evader_node == pursuer1_node or evader_node == pursuer2_node:
            return True
        # 条件2: 逃逸者与任一追捕者直接相连
        if (
            self.graph.adj_matrix[evader_node][pursuer1_node] == 1
            or self.graph.adj_matrix[evader_node][pursuer2_node] == 1
        ):
            return True
        return False

    def calculate_predicted_move(
        self, evader_node, pursuer1_node, pursuer2_node
    ):  # 预测若干步后的目标位置
        if self.is_captured(evader_node, pursuer1_node, pursuer2_node):
            return evader_node, pursuer1_node, pursuer2_node

        evader_target, pursuer1_target, pursuer2_target = (
            evader_node,
            pursuer1_node,
            pursuer2_node,
        )
        # 逃逸者若干步后的策略
        e_prd, p1_prd, p2_prd = evader_node, pursuer1_node, pursuer2_node
        for i in range(self.predict_step):
            e_prd, _, _ = self.calculate_next_move(e_prd, p1_prd, p2_prd)
        evader_target = e_prd
        # 追捕者1若干步后的策略
        e_prd, p1_prd, p2_prd = evader_node, pursuer1_node, pursuer2_node
        for i in range(self.predict_step):
            _, p1_prd, _ = self.calculate_next_move(e_prd, p1_prd, p2_prd)
        pursuer1_target = p1_prd
        # 追捕者2若干步后的策略
        e_prd, p1_prd, p2_prd = evader_node, pursuer1_node, pursuer2_node
        for i in range(self.predict_step):
            _, _, p2_prd = self.calculate_next_move(e_prd, p1_prd, p2_prd)
        pursuer2_target = p2_prd

        return evader_target, pursuer1_target, pursuer2_target


class RobotController:
    def __init__(self, robot_id, robot_name, role, client_socket, addr, server):
        """新增server参数用于事件通知"""
        self.robot_id = robot_id
        self.robot_name = robot_name
        self.role = role  # 'evader' 或 'pursuer'
        self.client_socket = client_socket
        self.addr = addr
        self.current_position = None  # 实际坐标
        self.current_node = None  # 最近的节点
        self.target_position = None  # 目标坐标
        self.target_node = None  # 目标节点
        self.last_update_time = time.time()
        self.last_heartbeat_time = time.time()
        self.status = "connecting"  # connecting, active, paused, disconnected
        self.active = False
        self.debug_mode = False
        self.server = server  # 保存服务器实例引用
        self.last_target_send_time = time.time()  # [新增] 初始化最后发送航点时间
        # 启动接收线程
        self.receive_thread = threading.Thread(
            target=self.receive_messages, daemon=True
        )
        self.receive_thread.start()
        # 发送角色分配信息
        self.send_role_assignment()

    def build_frame(self, command, payload=None):
        """构建32字节通信帧 (修正后的协议)"""
        if payload is None:
            payload = bytes(14)  # 默认14字节负载
        elif len(payload) > 14:
            payload = payload[:14]  # 截断超长部分
        elif len(payload) < 14:
            payload = payload.ljust(14, b"\x00")  # 填充不足部分
        # 获取当前时间戳(毫秒)
        timestamp = int(time.time() * 1000)
        # 构建帧头部分 (1B版本 + 1B命令 + 4B机器人ID + 8B时间戳)
        header = struct.pack(">BBiQ", VERSION, command, self.robot_id, timestamp)
        frame_data = header + payload
        # 计算CRC32校验码 (对前28字节)
        crc32 = zlib.crc32(frame_data) & 0xFFFFFFFF
        crc_bytes = struct.pack(">I", crc32)
        full_frame = frame_data + crc_bytes
        # 调试输出
        if self.debug_mode:
            logger.debug(
                f"[Robot-{self.robot_id}] 构建帧: CMD={command}, TS={timestamp}"
            )
            logger.debug(f"  Header: {binascii.hexlify(header)}")
            logger.debug(f"  Payload: {binascii.hexlify(payload)}")
            logger.debug(f"  CRC: {crc_bytes.hex()}")
        return full_frame

    def parse_frame(self, data):
        """解析32字节通信帧 (修正后的协议)"""
        if len(data) != FRAME_SIZE:
            logger.error(f"无效帧长度: {len(data)}字节 (预期{FRAME_SIZE}字节)")
            return None
        # 分离数据和校验码
        frame_data = data[:28]  # 前28字节用于CRC计算
        received_crc = struct.unpack(">I", data[28:32])[0]
        # 验证CRC
        calculated_crc = zlib.crc32(frame_data) & 0xFFFFFFFF
        if received_crc != calculated_crc:
            logger.warning(
                f"CRC校验失败! 接收: {received_crc:08X}, 计算: {calculated_crc:08X}"
            )
            if self.debug_mode:
                logger.debug(f"原始数据: {binascii.hexlify(data)}")
                logger.debug(f"帧数据: {binascii.hexlify(frame_data)}")
            return None
        # 解析帧头
        try:
            # 关键修复：只解析前14字节作为帧头
            header_part = frame_data[:14]
            version, command, robot_id, timestamp = struct.unpack(">BBiQ", header_part)
            payload = frame_data[14:28]  # 接下来的14字节是负载
            # 验证协议版本
            if version != VERSION:
                logger.warning(f"协议版本不匹配! 接收: {version}, 预期: {VERSION}")
                return None
            return {
                "version": version,
                "command": command,
                "robot_id": robot_id,
                "timestamp": timestamp,
                "payload": payload,
            }
        except struct.error as e:
            logger.error(f"帧解析错误: {e}")
            return None

    def recv_exact(self, size):
        """接收指定长度的数据，超时返回空字节而不是None"""
        data = b""
        start_time = time.time()
        while len(data) < size:
            try:
                self.client_socket.settimeout(1.0)  # 设置每次接收的超时
                chunk = self.client_socket.recv(size - len(data))
                if not chunk:
                    return None  # 连接真正断开
                data += chunk
            except socket.timeout:
                # 检查总时间是否超过10秒
                if time.time() - start_time > 10:
                    logger.warning(f"接收数据超时超过10秒，断开连接")
                    return None
                # 否则继续等待
                continue
            except BlockingIOError:
                # 非阻塞错误继续等待
                if time.time() - start_time > 10:
                    return None
                time.sleep(0.1)
        return data

    def send_role_assignment(self):
        """发送角色分配信息"""
        role_code = ROLES[self.role]
        payload = struct.pack(">B", role_code) + bytes(13)  # 角色 + 保留 (共14字节)
        frame = self.build_frame(COMMAND_TYPES["RoleAssignment"], payload)
        try:
            self.client_socket.sendall(frame)
            logger.info(f"[Robot-{self.robot_id}] 已分配角色: {self.role}")
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送角色失败: {e}")
            return False

    def update_position(self, x, y):
        """更新位置并检测节点变化"""
        self.current_position = (x, y)
        old_node = self.current_node
        self.current_node = self.server.graph.position_to_node(
            x, y
        )  # 使用服务器的graph实例
        self.last_update_time = time.time()
        self.status = "active"
        self.last_heartbeat_time = time.time()
        # 如果节点号变化且当前机器人处于活跃状态，触发策略计算
        if old_node != self.current_node and self.is_active():
            self.server.on_robot_node_changed()  # 通知服务器节点变化

    def update_target(self, target_node, graph):
        self.target_node = target_node
        self.target_position = graph.node_to_position(target_node)
        self.last_update_time = time.time()

    def send_target(self, graph):
        """发送目标位置到客户端"""
        if self.target_position is None or self.client_socket is None:
            return False
        try:
            x, y = self.target_position
            # 负载调整为12字节 (3个float) + 2字节填充
            payload = struct.pack(">fff", x, y, 30) + bytes(2)  # x, y, timeout
            frame = self.build_frame(COMMAND_TYPES["NavigationGoal"], payload)
            self.client_socket.sendall(frame)
            self.last_target_send_time = time.time()  # [修改] 发送成功后更新时间
            # logger.info(f"[Robot-{self.robot_id}] 发送目标: ({x:.2f}, {y:.2f})")
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送目标失败: {e}")
            self.status = "disconnected"
            # 注意：如果发送失败，不更新 last_target_send_time，以便更快重试
            return False

    def send_heartbeat(self):
        """发送心跳包到客户端"""
        if self.client_socket is None:
            return False
        try:
            seq = int(time.time() * 1000) % 1000000
            payload = struct.pack(">i", seq) + bytes(10)  # 序列号 + 保留 (共14字节)
            frame = self.build_frame(COMMAND_TYPES["Heartbeat"], payload)
            self.client_socket.sendall(frame)
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送心跳失败: {e}")
            self.status = "disconnected"
            return False

    def receive_messages(self):
        """接收来自客户端的消息"""
        try:
            while self.status != "disconnected":
                # 接收完整帧
                data = self.recv_exact(FRAME_SIZE)
                if not data:
                    logger.info(f"[Robot-{self.robot_id}] 连接断开")
                    self.status = "disconnected"
                    break
                if data == b"":  # 超时但未断开
                    # 检查心跳超时
                    if time.time() - self.last_heartbeat_time > ROBOT_TIMEOUT:
                        logger.warning(f"[Robot-{self.robot_id}] 心跳超时，断开连接")
                        self.status = "disconnected"
                        break
                    continue
                # 解析数据帧
                frame = self.parse_frame(data)
                if not frame:
                    continue
                # 验证机器人ID
                if frame["robot_id"] != self.robot_id:
                    logger.warning(
                        f"[Robot-{self.robot_id}] 收到错误ID的帧: {frame['robot_id']}"
                    )
                    continue
                # 处理心跳包
                if frame["command"] == COMMAND_TYPES["Heartbeat"]:
                    self.last_heartbeat_time = time.time()
                    # 发送心跳响应
                    self.send_heartbeat()
                    continue
                # 处理位置上报 - 修正为与多机巡线服务器一致的格式
                if frame["command"] == COMMAND_TYPES["PositionReport"]:
                    # 解析位置数据 (x, y, status) - 使用12字节
                    try:
                        # 修改点：使用x, y, status而非x, y, theta
                        x, y, status = struct.unpack(">fff", frame["payload"][:12])
                        self.update_position(x, y)
                        logger.debug(
                            f"[Robot-{self.robot_id}] 位置更新: ({x:.2f},{y:.2f}), 状态: {status}"
                        )
                    except struct.error as e:
                        logger.error(f"[Robot-{self.robot_id}] 位置报告解析错误: {e}")
                        continue
                # 处理指令确认
                elif frame["command"] == COMMAND_TYPES["CommandAck"]:
                    try:
                        acked_command, status_code = struct.unpack(
                            ">BB", frame["payload"][:2]
                        )
                        status_msg = next(
                            (k for k, v in STATUS_CODES.items() if v == status_code),
                            "Unknown",
                        )
                        logger.debug(
                            f"[Robot-{self.robot_id}] 收到指令确认: 命令={acked_command}, 状态={status_msg}"
                        )
                    except struct.error as e:
                        logger.warning(f"[Robot-{self.robot_id}] 指令确认解析失败: {e}")
                else:
                    logger.warning(
                        f"[Robot-{self.robot_id}] 收到未知指令: {frame['command']}"
                    )
        except (ConnectionResetError, socket.error) as e:
            logger.warning(f"[Robot-{self.robot_id}] 连接异常: {e}")
            self.status = "disconnected"
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 接收消息错误: {e}")
            self.status = "disconnected"
        finally:
            if self.client_socket:
                self.client_socket.close()
                self.client_socket = None

    def get_info(self):
        pos_str = (
            "未知"
            if self.current_position is None
            else f"({self.current_position[0]:.1f}, {self.current_position[1]:.1f})"
        )
        target_str = (
            "未设置"
            if self.target_position is None
            else f"({self.target_position[0]:.1f}, {self.target_position[1]:.1f})"
        )
        node_str = "未映射" if self.current_node is None else str(self.current_node)
        target_node_str = (
            "未设置" if self.target_node is None else str(self.target_node)
        )
        return {
            "id": self.robot_id,
            "name": self.robot_name,
            "role": self.role,
            "position": pos_str,
            "target": target_str,
            "node": node_str,
            "target_node": target_node_str,
            "status": self.status,
            "last_update": time.time() - self.last_update_time,
            "last_heartbeat": time.time() - self.last_heartbeat_time,
        }

    def is_active(self):
        """检查机器人是否活跃（连接且有位置更新）"""
        if self.status == "disconnected":
            return False
        # 检查心跳超时
        if time.time() - self.last_heartbeat_time > ROBOT_TIMEOUT * 2:
            self.status = "disconnected"
            return False
        return (
            self.status == "active"
            and (time.time() - self.last_update_time) < ROBOT_TIMEOUT
        )

    def disconnect(self):
        """断开机器人连接 (简化处理)"""
        self.status = "disconnected"
        if self.client_socket:
            try:
                self.client_socket.shutdown(socket.SHUT_RDWR)
            except:
                pass
            self.client_socket.close()
            self.client_socket = None
        # 注意：receive_thread 会因 socket 关闭而自然退出，无需显式 join


class MultiRobotServer:
    def __init__(self):
        logger.info("程序启动...")
        logger.info("开始加载图文件...")
        self.graph = GraphManager()
        logger.info("图文件加载完毕，开始加载策略文件...")
        self.game_strategy = GameStrategy(self.graph)
        logger.info("策略文件加载完毕，开始通讯初始化...")
        # 机器人存储
        self.evader = None  # 逃逸者
        self.pursuers = {}  # 追捕者字典: pursuer_id -> RobotController
        self.server_socket = None
        # 游戏状态
        self.game_active = False
        self.game_thread = None
        # 心跳管理
        self.heartbeat_thread = threading.Thread(
            target=self.heartbeat_loop, daemon=True
        )
        self.heartbeat_thread.start()
        # 添加节点变化事件
        self.node_change_event = threading.Event()  # 节点变化事件
        # [新增] 连接管理锁
        self.connection_lock = threading.Lock()
        logger.info("通讯初始化完毕")

    def on_robot_node_changed(self):
        """当机器人节点变化时调用此方法"""
        self.node_change_event.set()  # 触发事件

    def start(self):
        # 创建TCP服务器套接字
        logger.info("正在尝试启动通信接口...")
        logger.info(f"服务器IP: {SERVER_IP}, 端口: {SERVER_PORT}")
        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.server_socket.bind((SERVER_IP, SERVER_PORT))
        self.server_socket.listen(5)
        logger.info(f"[Game Server] 正在监听 {SERVER_IP}:{SERVER_PORT}...")
        # 启动状态监控线程
        threading.Thread(target=self.monitor_robots_status, daemon=True).start()
        # 启动游戏主循环
        self.game_thread = threading.Thread(target=self.run_game_loop, daemon=True)
        self.game_thread.start()
        # 接受客户端连接
        self.accept_connections()

    def get_all_robots_info(self):
        """获取所有机器人的状态信息"""
        robots = []
        if self.evader:
            robots.append(self.evader.get_info())
        for pursuer in self.pursuers.values():
            robots.append(pursuer.get_info())
        return robots

    def recv_exact(self, sock, size):
        """接收指定长度的数据，超时返回空字节而不是None"""
        data = b""
        start_time = time.time()
        while len(data) < size:
            try:
                sock.settimeout(1.0)  # 设置每次接收的超时
                chunk = sock.recv(size - len(data))
                if not chunk:
                    return None  # 连接真正断开
                data += chunk
            except socket.timeout:
                # 检查总时间是否超过10秒
                if time.time() - start_time > 10:
                    logger.warning(f"接收连接请求超时超过10秒，断开连接")
                    return None
                # 否则继续等待
                continue
        return data

    def parse_frame(self, data):
        """解析32字节通信帧 (修正后的协议)"""
        if len(data) != FRAME_SIZE:
            logger.error(f"无效帧长度: {len(data)}字节 (预期{FRAME_SIZE}字节)")
            return None
        # 分离数据和校验码
        frame_data = data[:28]  # 前28字节用于CRC计算
        received_crc = struct.unpack(">I", data[28:32])[0]
        # 验证CRC
        calculated_crc = zlib.crc32(frame_data) & 0xFFFFFFFF
        if received_crc != calculated_crc:
            logger.warning(
                f"CRC校验失败! 接收: {received_crc:08X}, 计算: {calculated_crc:08X}"
            )
            return None
        # 解析帧头
        try:
            # 关键修复：只解析前14字节作为帧头
            header_part = frame_data[:14]
            version, command, robot_id, timestamp = struct.unpack(">BBiQ", header_part)
            payload = frame_data[14:28]  # 接下来的14字节是负载
            # 验证协议版本
            if version != VERSION:
                logger.warning(f"协议版本不匹配! 接收: {version}, 预期: {VERSION}")
                return None
            return {
                "version": version,
                "command": command,
                "robot_id": robot_id,
                "timestamp": timestamp,
                "payload": payload,
            }
        except struct.error as e:
            logger.error(f"帧解析错误: {e}")
            return None

    def build_frame(self, command, payload=None, robot_id=0):
        """构建通信帧的通用方法"""
        if payload is None:
            payload = bytes(14)
        elif len(payload) > 14:
            payload = payload[:14]
        elif len(payload) < 14:
            payload = payload.ljust(14, b"\x00")
        timestamp = int(time.time() * 1000)
        header = struct.pack(">BBiQ", VERSION, command, robot_id, timestamp)
        frame_data = header + payload
        crc32 = zlib.crc32(frame_data) & 0xFFFFFFFF
        crc_bytes = struct.pack(">I", crc32)
        return frame_data + crc_bytes

    def accept_connections(self):
        """接受并处理新客户端连接"""
        while True:
            try:
                client_socket, addr = self.server_socket.accept()
                logger.info(f"[Game Server] 新连接来自: {addr}")
                # 设置接收超时
                client_socket.settimeout(2.0)
                # 1. 接收连接请求帧
                data = self.recv_exact(client_socket, FRAME_SIZE)
                if not data:
                    logger.warning("连接未发送数据")
                    client_socket.close()
                    continue
                # 2. 解析连接请求
                frame = self.parse_frame(data)
                if not frame or frame["command"] != COMMAND_TYPES["ConnectionRequest"]:
                    logger.warning("无效的连接请求")
                    client_socket.close()
                    continue
                # 3. 获取机器人名称
                try:
                    robot_name = frame["payload"].rstrip(b"\x00").decode("utf-8")
                except UnicodeDecodeError:
                    robot_name = "unknown_robot"
                logger.info(f"连接请求 | 机器人名称: {robot_name}")
                # 4. 分配机器人ID和角色
                # --- 修改开始：基于槽位是否被占用进行分配 ---
                role, robot_id = None, None
                # 检查逃逸者槽位 (ID 0)
                if self.evader is None:
                    # 如果尚未分配逃逸者，则分配
                    role, robot_id = "evader", 0
                else:
                    # 逃逸者槽位已存在，尝试分配追捕者
                    # 检查追捕者1槽位 (ID 1)
                    if 1 not in self.pursuers:
                        role, robot_id = "pursuer", 1
                    # 检查追捕者2槽位 (ID 2)
                    elif 2 not in self.pursuers:
                        role, robot_id = "pursuer", 2
                    else:
                        # 所有槽位都被占用
                        logger.warning("拒绝连接：无可用位置 (所有角色槽位已满)")
                        client_socket.close()
                        continue
                # --- 修改结束 ---
                if (
                    robot_id is None
                ):  # 这个检查实际上现在不会触发，因为上面逻辑保证了要么分配要么continue
                    logger.warning("拒绝连接：无可用位置")
                    client_socket.close()
                    continue
                # 5. 发送连接响应
                response_payload = struct.pack(">i", robot_id) + bytes(
                    10
                )  # 4字节ID + 10字节填充
                response_frame = self.build_frame(
                    COMMAND_TYPES["ConnectionResponse"], response_payload, robot_id
                )
                client_socket.sendall(response_frame)
                logger.info(f"发送连接响应，分配ID: {robot_id}, 角色: {role}")
                # 6. 创建机器人控制器（新增server参数）
                robot = RobotController(
                    robot_id, robot_name, role, client_socket, addr, self
                )
                # --- 修改：简化连接覆盖逻辑 ---
                # 直接根据分配的角色和ID存储，旧连接的清理由 monitor_robots_status 负责
                # 这可以避免在连接分配过程中因调用 disconnect 而可能产生的竞争或阻塞
                if role == "evader":
                    self.evader = robot
                else:  # role == "pursuer"
                    self.pursuers[robot_id] = robot
                # --- 修改结束 ---
                logger.info(f"机器人 {robot_id} ({role}) 连接成功")
                self.check_game_state()
            except Exception as e:
                logger.error(f"接受连接错误: {e}")
                # 确保 client_socket 在出错时被关闭
                if "client_socket" in locals() and client_socket:
                    try:
                        client_socket.close()
                    except:
                        pass

    def check_game_state(self):
        """检查游戏状态并更新"""
        evader_ready = self.evader and self.evader.is_active()
        pursuer1_ready = 1 in self.pursuers and self.pursuers[1].is_active()
        pursuer2_ready = 2 in self.pursuers and self.pursuers[2].is_active()
        pursuers_ready = pursuer1_ready and pursuer2_ready
        if evader_ready and pursuers_ready:
            if not self.game_active:
                logger.info("所有机器人准备就绪，开始围捕!")
                self.game_active = True
                self.node_change_event.set()
        else:
            if self.game_active:
                logger.warning("机器人断开，暂停围捕!")
                self.game_active = False

    def run_game_loop(self):
        """游戏主循环 - 完全由事件驱动"""
        logger.info("[Game Loop] Game Loop 启动 (事件驱动模式)...")
        while True:
            # 等待节点变化事件
            self.node_change_event.wait()
            if self.game_active:
                self.calculate_and_send_strategy()
            # 重置事件，等待下一次触发
            self.node_change_event.clear()

    def calculate_and_send_strategy(self):
        # logger.info("[Game Loop] 进入策略计算和航点发送")
        if not self.game_active:
            return
        if not self.evader or not self.evader.current_position:
            logger.warning("逃逸者未连接或位置未知，跳过策略计算")
            return
        pursuer1 = self.pursuers.get(1)
        pursuer2 = self.pursuers.get(2)
        if (
            not pursuer1
            or not pursuer2
            or not pursuer1.current_position
            or not pursuer2.current_position
        ):
            logger.warning("追捕者未连接或位置未知，跳过策略计算")
            return
        # 获取当前节点
        evader_node = self.evader.current_node
        pursuer1_node = pursuer1.current_node
        pursuer2_node = pursuer2.current_node
        # 检查是否捕获
        if self.game_strategy.is_captured(evader_node, pursuer1_node, pursuer2_node):
            logger.info("逃逸者已被捕获！游戏结束")
            self.game_active = False
            self.game_strategy.capture_count += 1
            return
        # 计算下一步 (使用预测步长)
        evader_next, pursuer1_next, pursuer2_next = (
            self.game_strategy.calculate_predicted_move(
                evader_node, pursuer1_node, pursuer2_node
            )
        )
        # logger.info(f"[Game Loop] 计算下一步: 逃逸者={evader_next}, 追捕者1={pursuer1_next}, 追捕者2={pursuer2_next}")
        current_time = time.time()
        # [修改] 使用 NAV_GOAL_MAX_INTERVAL 进行强制更新判断
        # 逃逸者检查
        evader_force_update = (
            current_time - self.evader.last_target_send_time
        ) > NAV_GOAL_MAX_INTERVAL
        if evader_next != self.evader.target_node or evader_force_update:
            # logger.info(f"更新逃逸者目标节点为 {evader_next} (强制: {evader_force_update})")
            self.evader.update_target(evader_next, self.graph)
            self.evader.send_target(self.graph)
        # 追捕者1检查
        pursuer1_force_update = (
            current_time - pursuer1.last_target_send_time
        ) > NAV_GOAL_MAX_INTERVAL
        if pursuer1_next != pursuer1.target_node or pursuer1_force_update:
            # logger.info(f"更新追捕者1目标节点为 {pursuer1_next} (强制: {pursuer1_force_update})")
            pursuer1.update_target(pursuer1_next, self.graph)
            pursuer1.send_target(self.graph)
        # 追捕者2检查
        pursuer2_force_update = (
            current_time - pursuer2.last_target_send_time
        ) > NAV_GOAL_MAX_INTERVAL
        if pursuer2_next != pursuer2.target_node or pursuer2_force_update:
            # logger.info(f"更新追捕者2目标节点为 {pursuer2_next} (强制: {pursuer2_force_update})")
            pursuer2.update_target(pursuer2_next, self.graph)
            pursuer2.send_target(self.graph)
        # 输出是否更新
        # logger.info(f"[Game Loop] 策略更新计时器, 逃逸者{evader_force_update}, 追捕者1{pursuer1_force_update}, 追捕者2{pursuer2_force_update}")

    def heartbeat_loop(self):
        """定期发送心跳包"""
        while True:
            time.sleep(HEARTBEAT_INTERVAL)
            current_time = time.time()  # [新增] 获取当前时间用于比较
            # 给逃逸者发送心跳
            if self.evader and self.evader.status == "active":
                self.evader.send_heartbeat()
                # [新增] 检查逃逸者是否需要强制更新航点
                if (
                    current_time - self.evader.last_target_send_time
                    > NAV_GOAL_MAX_INTERVAL
                ):
                    logger.info(
                        f"[Heartbeat] 逃逸者 {self.evader.robot_id} 超时未更新航点，触发强制更新。"
                    )
                    # 在新线程中触发，避免阻塞心跳线程
                    threading.Thread(
                        target=self.calculate_and_send_strategy, daemon=True
                    ).start()

            # 给追捕者发送心跳
            for pursuer in self.pursuers.values():
                if pursuer.status == "active":
                    pursuer.send_heartbeat()
                    # [新增] 检查追捕者是否需要强制更新航点
                    if (
                        current_time - pursuer.last_target_send_time
                        > NAV_GOAL_MAX_INTERVAL
                    ):
                        logger.info(
                            f"[Heartbeat] 追捕者 {pursuer.robot_id} 超时未更新航点，触发强制更新。"
                        )
                        # 在新线程中触发，避免阻塞心跳线程
                        threading.Thread(
                            target=self.calculate_and_send_strategy, daemon=True
                        ).start()

    def monitor_robots_status(self):
        """监控所有机器人状态并定期打印"""
        while True:
            time.sleep(4)
            status_report = ["\n===== 机器人状态监控 ====="]
            status_report.append(
                f"游戏状态: {'进行中' if self.game_active else '暂停'}"
            )
            status_report.append(f"捕获次数: {self.game_strategy.capture_count}")
            status_report.append(f"预测步长: {PREDICT_STEP}")
            if self.evader:
                info = self.evader.get_info()
                status_report.append(f"逃逸者 {info['id']}:")
                status_report.append(
                    f"  位置: {info['position']} | 节点: {info['node']}"
                )
                status_report.append(
                    f"  目标: {info['target']} | 目标节点: {info['target_node']}"
                )
                status_report.append(
                    f"  状态: {info['status']} | 最后更新: {info['last_update']:.1f}秒前"
                )
                status_report.append(f"  最后心跳: {info['last_heartbeat']:.1f}秒前")
            else:
                status_report.append("逃逸者: 未连接")
            for robot_id in [1, 2]:  # 固定两个追捕者ID
                robot = self.pursuers.get(robot_id)
                if robot:
                    info = robot.get_info()
                    status_report.append(f"追捕者 {info['id']}:")
                    status_report.append(
                        f"  位置: {info['position']} | 节点: {info['node']}"
                    )
                    status_report.append(
                        f"  目标: {info['target']} | 目标节点: {info['target_node']}"
                    )
                    status_report.append(
                        f"  状态: {info['status']} | 最后更新: {info['last_update']:.1f}秒前"
                    )
                    status_report.append(
                        f"  最后心跳: {info['last_heartbeat']:.1f}秒前"
                    )
                else:
                    status_report.append(f"追捕者 {robot_id}: 未连接")
            status_report.append("=" * 30)
            logger.info("\n".join(status_report))
            # 清理断开连接的机器人
            if (
                self.evader
                and not self.evader.is_active()
                and self.evader.status == "disconnected"
            ):
                self.evader = None
                logger.info("[清理] 逃逸者已断开连接")
            to_remove = []
            for robot_id, robot in self.pursuers.items():
                if not robot.is_active() and robot.status == "disconnected":
                    to_remove.append(robot_id)
            for robot_id in to_remove:
                del self.pursuers[robot_id]
                logger.info(f"[清理] 追捕者 {robot_id} 已断开连接")
            # 更新游戏状态
            self.check_game_state()


import sys
from qt_app_ui_pur import PursuitGameUI
from PyQt5.QtWidgets import QApplication

if __name__ == "__main__":
    server = MultiRobotServer()
    import threading

    server_thread = threading.Thread(target=server.start)
    server_thread.daemon = True
    server_thread.start()
    # 根据全局开关决定是否启动Qt窗口
    if IF_VISUALIZE:
        # 启动UI
        app = QApplication(sys.argv)
        ui = PursuitGameUI(server=server, logger=logger)
        ui.show()
        app.exec_()
    else:
        logger.info("Qt可视化窗口已禁用 (IF_VISUALIZE=False)")
        try:
            # 主线程保持运行
            while True:
                time.sleep(0.5)
        except KeyboardInterrupt:
            logger.info("服务器已停止")
